#include "problem.h"

#include <Eigen/Geometry>
#include <ceres/ceres.h>
#include <ceres/rotation.h>

using namespace ceres;

//相似变换残差
struct SimilaTransResidual {
    SimilaTransResidual(const Vector3d Xm, const Vector3d Xw) : Xm(Xm), Xw(Xw) {}

    template <typename T> bool operator()(const T *lambda, const T *r, const T *t, T *residual) const {
        T Xm_[3];
        T Pm[3];
        Xm_[0] = T(Xm.x());
        Xm_[1] = T(Xm.y());
        Xm_[2] = T(Xm.z());
        ceres::AngleAxisRotatePoint(r, Xm_, Pm);
        residual[0] = Xw[0] - (lambda[0] * Pm[0] + t[0]);
        residual[1] = Xw[1] - (lambda[0] * Pm[1] + t[1]);
        residual[2] = Xw[2] - (lambda[0] * Pm[2] + t[2]); //因为地面坐标是左手系? 这里要用减法

        return true;
    }

private:
    Vector3d Xm, Xw;
};
DataIntersection absolute_orientation(const DataAbsoluteOrientation &data_ao) {

    DataIntersection result;

    // 1. 利用同名点数据，生成相对定向结果
    Vector3d B;
    Matrix3d Rr;
    DataRelativeOrientation data_ro;
    data_ro.Bx = data_ao.Bx;
    data_ro.tiepoints = data_ao.tiepoints;
    std::tie(B, Rr) = relative_orientation(data_ro);

    // 2. 根据相对定向结果和同名点，计算模型坐标，即在第一个影像坐标系下进行前方交会
    std::vector<Vector3d> model_points(data_ao.tiepoints.size());
    //前方交会参数
    DataIntersection data;
    {
        data.R_eo[0] = Matrix3d::Identity();
        data.r_eo[0] = Vector3d::Zero();
        data.X_eo[0] = Vector3d::Zero();
        data.R_eo[1] = Rr;
        AngleAxisd ra(Rr);
        data.r_eo[1] = ra.angle() * ra.axis();
        data.X_eo[1] = B;
    }
    //计算前方交会计算相对外方位元素
    for (int i = 0; i < model_points.size(); i++) {
        data.p_img[0] = data_ao.tiepoints[i].first;
        data.p_img[1] = data_ao.tiepoints[i].second;
        Vector3d X_initial = intersection_similarity_transform(data);
        model_points[i] = intersection_least_squares(data, X_initial);
    }

    // 3. 根据前方交会的模型坐标和控制点，计算旋转，缩放，平移参数
    double s = 1.0;
    Vector3d ra = Vector3d::Zero();
    Vector3d T = Vector3d::Zero();

    // 3.1 计算尺度、缩放、平移的初始值

    // 3.2 构造最小二乘优化，利用控制点和模型坐标，计算7参数
    ceres::Problem problem;
    std::vector<ceres::ResidualBlockId> blocks;
    for (int i = 0; i < data_ao.tiepoints.size(); i++) {
        ceres::CostFunction *cost = new ceres::AutoDiffCostFunction<SimilaTransResidual, 3, 1, 3, 3>(
            new SimilaTransResidual(model_points[i], data_ao.gcps[i]));
        // std::cout<<"model"<<model_points[i]<<std::endl;
        // std::cout<<"gcps"<<std::endl<< data_ao.gcps[i]<<std::endl;
        // std::cout<<"-------------------"<<std::endl;
        problem.AddResidualBlock(cost, nullptr, &s, ra.data(), T.data());
    }

    ceres::Solver::Options op;

    op.max_num_iterations = 100;
    op.linear_solver_type = ceres::DENSE_QR;
    ceres::Solver::Summary summary;
    ceres::Solve(op, &problem, &summary);
    // std::cout << summary.BriefReport() << "\n";
    // std::cout << T << "\n";
    //  4. 根据 7 参数信息，生成最后的外方位元素
    result.R_eo[0] = rodrigues(ra);
    result.X_eo[0] = T;
    result.R_eo[1] = rodrigues(ra) * Rr;
    result.X_eo[1] = T + s * rodrigues(ra) * B;

    return result;
}
